#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include "MapFix.h"

ros::Publisher mapfix_pub;
CMapFix map_fix;

// 机器人GPS回调函数
void GPSRobotCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
  //ROS_WARN("-----");
  double latitude = msg->latitude;
  double longitude = msg->longitude;
  double altitude = msg->altitude;
  //ROS_INFO("[机器人的GPS]  纬度= %f    经度= %f",latitude,longitude);

  double dDstLat = 0;
  double dDstLong = 0;
  map_fix.GPSFix(latitude,longitude,dDstLat,dDstLong);
  //ROS_WARN("[换算手机GPS]  纬度= %f    经度= %f",dDstLat,dDstLong);
  sensor_msgs::NavSatFix new_msg;
  new_msg = *msg;
  new_msg.latitude = dDstLat;
  new_msg.longitude = dDstLong;
  mapfix_pub.publish(new_msg);
}

int main(int argc, char** argv)
{
  setlocale(LC_ALL,"");
  ros::init(argc, argv, "test_node");
  ros::NodeHandle n;

  ros::Subscriber gps_robot_sub = n.subscribe("/ublox_gps/fix", 10, &GPSRobotCallback);
  mapfix_pub = n.advertise<sensor_msgs::NavSatFix>("/ublox_gps/mapfix",10);

  ros::Rate r(30);
  while(ros::ok())
  {
    ros::spinOnce();
    r.sleep();
  }

  return 0;
}